#include <iostream>
#include <string>
// 包含SpireMS SDK头文件
#include <sms_core.h>


class ShareMemReceiverTest
{
public:
    ShareMemReceiverTest() : _sub(
        "/flyeagle/lidar_front_cp", 
        "memory_msgs::PointCloud", 
        std::bind(&ShareMemReceiverTest::callback, this, std::placeholders::_1)
    ),
    _pub(
        "/flyeagle/lidar_front", 
        "memory_msgs::PointCloud"
    )
    {
        img_cnt = 0;
        dt_tot = 0.0;
        print_dt = true;
    }
    void callback(nlohmann::json msg)
    {
        // std::cout << "  " << sms::get_time_sec() - msg["timestamp"].get<double>() << std::endl;
        cv::Mat cvimg = sms::sms2pcl(msg);
        
        nlohmann::json msg2 = _pub.pcl2sms_mem(cvimg, msg["fields"], "lidar", msg["timestamp"]);
        _pub.publish(msg2);
        // std::cout << sms::get_time_sec() - msg["timestamp"].get<double>() << std::endl;
        
    }
    void join()
    {
        this->_sub.join();
    }

private:
    sms::Subscriber _sub;
    sms::Publisher _pub;
    int img_cnt;
    double dt_tot;
    bool print_dt;
};


int main(int argc, char *argv[])
{
    sms::mem_init();

    ShareMemReceiverTest tt;
    tt.join();
    return 0;
}

